/*
 * RemoteControl.cpp
 *
 *  Created on: Jan 16, 2014
 *      Author: ppks
 */

#include "common.h"
#include "RemoteControl.h"
#include "FlightController.h"
#include "FlightController_Failsafe.h"

/******************************************************************************/
/* Constants Definition                                                       */
/******************************************************************************/
/* Define channels masks that have to be checked for failsafe */
#ifndef RC_FAILSAFE_CHANNEL_MASK
#define RC_FAILSAFE_CHANNEL_MASK    ((1 << THROTTLEPIN) + (1 << YAWPIN) + (1 << ROLLPIN) + (1 << PITCHPIN))
#endif /* #ifndef RC_FAILSAFE_CHANNEL_MASK */

/******************************************************************************/
/* Global Variables Declaration                                               */
/******************************************************************************/
volatile uint16_t rcValue[RC_CHANNELS_NUM] = {1502, 1502, 1502, 1502, 1502, 1502, 1502, 1502};
static uint8_t rcChannel[RC_CHANNELS_NUM]  = {ROLLPIN, PITCHPIN, YAWPIN, THROTTLEPIN, AUX1PIN, AUX2PIN, AUX3PIN, AUX4PIN};

/* if this slows the PCINT readings we can switch to a define for each pcint bit */
static uint8_t PCInt_RX_Pins[PCINT_PIN_COUNT] = {PCINT_RX_BITS};

uint8_t rcSerialCount = 0;
int16_t rcSerial[RC_CHANNELS_NUM];          /* interval [1000;2000] - is rcData coming from MSP */

static uint8_t rcGoodChannelMask = 0;       /* This variable will hold masks for channels that received the correct data */

/******************************************************************************/
/* Local Functions Declaration                                                */
/******************************************************************************/
static uint16_t RC_ReadRawChannelData(uint8_t chan);

/******************************************************************************/
/* Function Code                                                              */
/******************************************************************************/
/*******************************************************************************
* Function Name: RC_ConfigureReceiver
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void RC_ConfigureReceiver(void)
{
  /* Configure each RC pin for PCINT */
#if (REMOTE_CONTROL_RECEIVER_TYPE == RC_TYPE_STANDARD_RX)
    #if defined(MEGA)
          DDRK = 0;  /* defined PORTK as a digital port ([A8-A15] are considered as digital PINs and not analogical) */
    #endif

    /* PCINT activation */
    for(uint8_t i = 0; i < PCINT_PIN_COUNT; i++)
    {
      PCINT_RX_PORT |= PCInt_RX_Pins[i];
      PCINT_RX_MASK |= PCInt_RX_Pins[i];
    }

    PCICR = PCIR_PORT_BIT;
#endif /* REMOTE_CONTROL_RECEIVER_TYPE == RC_TYPE_STANDARD_RX */
}


/*******************************************************************************
* Macro: RX_PIN_CHECK
********************************************************************************
* Summary:
*  predefined PC pin block (thanks to lianj)  - Version without failsafe
*
*******************************************************************************/
#define RX_PIN_CHECK(pin_pos, rc_value_pos)                 \
if (mask & PCInt_RX_Pins[pin_pos])                          \
{                                                           \
    if (!(pin & PCInt_RX_Pins[pin_pos]))                    \
    {                                                       \
        dTime = cTime - edgeTime[pin_pos];                  \
        if ((RC_MIN_PULSE_VALUE < dTime) &&                 \
            (dTime < RC_MAX_PULSE_VALUE))                   \
        {                                                   \
            rcValue[rc_value_pos] = dTime;                  \
            /* Failsafe Option */                           \
            if (dTime > FAILSAFE_RC_DETECT_TRESHOLD)        \
            {                                               \
                /* Always set mask for good channels */     \
                rcGoodChannelMask |= (1 << rc_value_pos);   \
            }                                               \
        }                                                   \
    }                                                       \
    else                                                    \
    {                                                       \
        edgeTime[pin_pos] = cTime;                          \
    }                                                       \
}

/*******************************************************************************
* ISR: RX_PC_INTERRUPT
********************************************************************************
* Summary:
*  this ISR is common to every receiver channel, it is call everytime a state
*  change occurs on any RX input pin
*
*******************************************************************************/
ISR (RX_PC_INTERRUPT)
{
    uint8_t mask;
    uint8_t pin;
    uint16_t cTime;
    uint16_t dTime;
    static uint16_t edgeTime[8];
    static uint8_t  PCintLast;

    pin = RX_PCINT_PIN_PORT;  /* RX_PCINT_PIN_PORT indicates the state of each PIN for the Arduino port dealing with Ports digital pins */

    mask = pin ^ PCintLast;   /* doing a ^ between the current interruption and the last one indicates which pin changed */
    cTime = micros();         /* micros() return a uint32_t, but it is not useful to keep the whole bits => we keep only 16 bits */
    sei();                    /* re enable other interrupts at this point, the rest of this interrupt
                                 is not so time critical and can be interrupted safely */
    PCintLast = pin;          /* we memorize the current state of all PINs [D0-D7] */

    /* Check every pin for a state change */
    #if (PCINT_PIN_COUNT > 0)
      RX_PIN_CHECK(0,2);
    #endif
    #if (PCINT_PIN_COUNT > 1)
      RX_PIN_CHECK(1,4);
    #endif
    #if (PCINT_PIN_COUNT > 2)
      RX_PIN_CHECK(2,5);
    #endif
    #if (PCINT_PIN_COUNT > 3)
      RX_PIN_CHECK(3,6);
    #endif
    #if (PCINT_PIN_COUNT > 4)
      RX_PIN_CHECK(4,7);
    #endif
    #if (PCINT_PIN_COUNT > 5)
      RX_PIN_CHECK(5,0);
    #endif
    #if (PCINT_PIN_COUNT > 6)
      RX_PIN_CHECK(6,1);
    #endif
    #if (PCINT_PIN_COUNT > 7)
      RX_PIN_CHECK(7,3);
    #endif

#ifdef FAILSAFE
    if ((rcGoodChannelMask & RC_FAILSAFE_CHANNEL_MASK) == RC_FAILSAFE_CHANNEL_MASK)
    {
        /* If all of the failsafe checked channels have good value - clear the counter */
        rcGoodChannelMask = 0;
        /* Check if the RC failsafe mode was active */
        if (Failsafe_IsRCFailsafeActive())
        {
            /* Decrease failsafe counter by half of the activation delay time to avoid rapid
             * exit due to a possible glitch when the connection is still bad. This will just slow down
             * the process of returning to normal operation */
            Failsafe_DecreaseRCCounter();
        }
        else
        {
            /* If the counter is below failsafe activation threshold - reset it */
            Failsafe_ClearRCCounter();
        }
    }
#endif /* FAILSAFE */
}

/*******************************************************************************
* Function Name: RC_ReadRawChannelData
********************************************************************************
* Summary:
*  this ISR is common to every receiver channel, it is call everytime a state
*  change occurs on any RX input pin
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
static uint16_t RC_ReadRawChannelData(uint8_t chan)
{
    uint16_t data;
    uint8_t oldSREG;

    oldSREG = SREG; cli();              /* Disable Interrupts Here */
    data = rcValue[rcChannel[chan]];    /* Copy the data Atomically */
    SREG = oldSREG;                     /* Restore interrupt state */

    return (data); /* We return the value correctly copied when the IRQ's where disabled */
}

/*******************************************************************************
* Function Name: RC_ProcessReceivedData
********************************************************************************
* Summary:
*  This function reads the RAW value of each channel in a safe mode (disabling
*  the interrupt) and stores them to the global variable rcData[]. Optionally
*  performs IIR filtering on the received data
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void RC_ProcessReceivedData(void)
{
    uint8_t chIndex;
    int16_t prevChData;
    int16_t currentChData;

    /* Loop Through Each Channel */
    for (chIndex = 0; chIndex < RC_CHANNELS_NUM; chIndex++)
    {
        prevChData = rcData[chIndex];
        currentChData = RC_ReadRawChannelData(chIndex);

#if RC_RECEIVER_IIR_FILTER_ENABLED
        if (abs(prevChData - currentChData) > RC_RECEIVER_IIR_THRESHOLD)
        {
            rcData[chIndex] = (prevChData * ((1 << RC_RECEIVER_IIR_FILTER_COEF) - 1) + currentChData) >> RC_RECEIVER_IIR_FILTER_COEF;
        }
        else
        {
            rcData[chIndex] = (prevChData * ((1 << RC_RECEIVER_IIR_SLOW_FILTER_COEF) - 1) + currentChData) >> RC_RECEIVER_IIR_SLOW_FILTER_COEF;
        }
#else /* RC_RECEIVER_IIR_FILTER_ENABLED */
        /* Record RAW Data into the output array */
        rcData[chIndex] = currentChData;
#endif /* RC_RECEIVER_IIR_FILTER_ENABLED */

        /* Jitter Filter */
        if (prevChData > rcData[chIndex])
        {
            rcData[chIndex]++;
        }
        else if (prevChData < rcData[chIndex])
        {
            rcData[chIndex]--;
        }

        /* rcData comes from MSP and overrides RX Data until rcSerialCount reaches 0 */
        if ((chIndex < RC_CHANNELS_NUM) && (rcSerialCount > 0))
        {
            rcSerialCount --;
#if defined(FAILSAFE)
            Failsafe_ClearRCCounter();
#endif /* defined(FAILSAFE) */

            /* only relevant channels are overridden */
            if (rcSerial[chIndex] > 900)
            {
                rcData[chIndex] = rcSerial[chIndex];
            }
        }
    }
}
